#ifndef UNIFORMGRID_H_
#define UNIFORMGRID_H_

#include "SpatialPartitioning.h"
#include "math/math.h"
#include "hmap.h"
#include "hset.h"
#include "RigidBody.h"
#include "CellCoordinates.h"
#include <list>
#include <boost/dynamic_bitset.hpp>

namespace physicsworks {

using math::vec3;

/**
 * A convenient struct that speed up the grid updating.
 */
struct RigidBodyMapping {
	
	RigidBodyMapping(list<RigidBodyMapping*>* l, list<RigidBodyMapping*>::iterator iter,
	                 const CellCoordinates& c, RigidBody* b)
	 : listPtr(l), iterator(iter), cellCoordinates(c), body(b) {}
	
	list<RigidBodyMapping*>* listPtr; /**< Points to the respective cell list */
	list<RigidBodyMapping*>::iterator iterator; /**< Position in the list */
	CellCoordinates cellCoordinates; /**< To which cell this body belongs */
	RigidBody* body; /**< The body */
};

class UniformGrid : public SpatialPartitioning {
public:
	UniformGrid(const vec3& a, const vec3& b, real cellDiameter, int numBuckets);
	~UniformGrid();
	list<pair<RigidBody*,RigidBody*> > getPotentialCollidingPairs();
	void add(RigidBody* b);
	void remove(RigidBody* b);
	void update();
	
private:
	typedef HMap<CellCoordinates, list<RigidBodyMapping*> >::Type CellHMap;
	typedef HMap<RigidBody*, RigidBodyMapping*>::Type RigidBodyHMap;
	typedef HSet<RigidBody*>::Type RigidBodyHSet;
	
	CellHMap cells; /**< The cells containing its respectives bodies */
	boost::dynamic_bitset<> occupiedCells; /**< Tells whether exist a body in a cell */
	list<CellCoordinates> occupiedCellsList; /**< Tells whether exist a body in a cell */
	
	RigidBodyHMap rigidBodies; /**< The rigid bodies inside the grid's boundaries */
	//list<RigidBody*> outRigidBodies; /**< The rigid bodies outside the grid's boundaries */
	RigidBodyHSet outRigidBodies; /**< The rigid bodies outside the grid's boundaries */
	real cellDiameter;
	vec3 max; /**< Max vertice of the grid */
	vec3 min; /**< Min vertice of the grid */
	
	int numCellsX;
	int numCellsY;
	int numCellsZ;
	
	/**
	 * Generates the cell's coordinates of box with respect of its
	 * corner (box.min.x, box.min.y, box.max.z). 
	 */
	CellCoordinates generateCellCoordinates(const vec3& corner) const {
		// transformed corner
		vec3 tcorner(corner.x - min.x, corner.y - min.y, corner.z - min.z);
		return CellCoordinates(static_cast<int>(tcorner.x / cellDiameter),
		                       static_cast<int>(tcorner.y / cellDiameter),
		                       static_cast<int>(tcorner.z / cellDiameter));
	}
	
	/**
	 * Is the box inside the grid's boundaries?
	 */
	bool isInsideGridBoundaries(const AABox& box) const {
		return box.min.x >= min.x && box.min.y >= min.y && box.min.z >= min.z &&
		       box.max.x <= max.x && box.max.y <= max.y && box.max.z <= max.z;
	}
	
	boost::dynamic_bitset<>::reference getOccupiedCellRef(const CellCoordinates& c) {
		return occupiedCells[c.x + numCellsX * c.y + (numCellsX + numCellsX * (numCellsY - 1)) * c.z];
	}
	
	void insert(RigidBody* b);
	void findPotentialCollidingPairs(list<pair<RigidBody*,RigidBody*> >& pl, RigidBody* body,
	                                 list<RigidBodyMapping*>::const_iterator beginIter,
	                                 list<RigidBodyMapping*>::const_iterator endIter, const CellCoordinates& cc);
};

}

#endif /*UNIFORMGRID_H_*/
